#include <Wire.h>
#include <Servo.h>
#include <EEPROM.h>
#include <Depecabot3.h>

Depecabot robot;

#define SERVO_INIT_POS 90

// Conjuntos de Entradas //
#define LEJOS_ATRAS 0
#define MEDIO_ATRAS 1
#define CERCA_ATRAS 2
#define CORRECTO 3
#define CERCA_ALANTE 4
#define MEDIO_ALANTE 5
#define LEJOS_ALANTE 6

// Conjuntos de Salida //
#define RETROCESO_RAPIDO -200
#define RETROCESO_MEDIO -30
#define RETROCESO_LENTO -5
#define PARADO 0
#define AVANCE_LENTO 5
#define AVANCE_MEDIO 30
#define AVANCE_RAPIDO 200

#define CONSIGNA 150

unsigned long timerLCD, timerGP2D;
char pos=90;
int distancia=0;
int ERROR=0;
int motor=0;
unsigned char Pertenencias[7]={0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
int Salidas[7]={0,0,0,0,0,0,0};

void Fuzzyfication(int X)
{
  char i;

  for(i=LEJOS_ATRAS;i<=LEJOS_ALANTE;i++)
     Pertenencias[i]=0;
     
  for(i=LEJOS_ATRAS;i<=LEJOS_ALANTE;i++)
  {
    if(X > (-30+(10*i)) && X <= (-20+(10*i)))
    {
        Pertenencias[i]=(-10*X+(-200+(100*i))); 
        Pertenencias[i+1]=(10*X+(300-(100*i)));
    }
  }
  
  if(X <= -30) Pertenencias[LEJOS_ATRAS]= 100;
  if(X >30) Pertenencias[LEJOS_ALANTE]=100; 
 /* 
  if(X > -20 && X <= -10){
      Pertenencias[MEDIO_ATRAS]=(-10*X-100); 
      Pertenencias[CERCA_ATRAS]=(10*X+200);
  }
  if(X > -10 && X <= 0){
      Pertenencias[CERCA_ATRAS]=(-10*X); 
      Pertenencias[CORRECTO]=(10*X+100);
  }
  if(X > 0 && X <= 10){
      Pertenencias[CORRECTO]=(-10*X+100); 
      Pertenencias[CERCA_ALANTE]=(10*X);
  }
  if(X > 10 && X <= 20){
      Pertenencias[CERCA_ALANTE]=(-10*X+200); 
      Pertenencias[MEDIO_ALANTE]=(10*X-100);
  }
  if(X > 20 && X <= 30){
      Pertenencias[CERCA_ALANTE]=(-10*X+300); 
      Pertenencias[LEJOS_ALANTE]=(10*X-200);
  } 
  */
  
}
  
int DeFuzzyfication()
{
  char i;
  long sumatorio_salidas=0, sumatorio_activacion=0;
  
  // Activaciçion de cadaregla, por su consecuente
  Salidas[0]=Pertenencias[LEJOS_ATRAS]*RETROCESO_RAPIDO;
  Salidas[1]=Pertenencias[MEDIO_ATRAS]*RETROCESO_MEDIO;
  Salidas[2]=Pertenencias[CERCA_ATRAS]*RETROCESO_LENTO;
  Salidas[3]=Pertenencias[CORRECTO]*PARADO;
  Salidas[4]=Pertenencias[CERCA_ALANTE]*AVANCE_LENTO;
  Salidas[5]=Pertenencias[MEDIO_ALANTE]*AVANCE_MEDIO;
  Salidas[6]=Pertenencias[LEJOS_ALANTE ]*AVANCE_RAPIDO;
    
  for(i=0;i<7;i++)
  {
    sumatorio_salidas+=Salidas[i];
    sumatorio_activacion+=Pertenencias[i];
  }
 
  return (sumatorio_salidas/sumatorio_activacion); 
}






void setup() 
{
  robot.init ();
  robot.servo1.attach(SERVO);            //Inicializacion servo
  robot.servo1.write(SERVO_INIT_POS);
  robot.lcd.clear();
}

void loop() 
{
  robot.lcd.setCursor(0,0);
  if(millis()-timerLCD>200)
  {
    timerLCD=millis();
    robot.lcd.clear();
    robot.lcd.setCursor(10,0);
    robot.lcd.print(distancia);
    robot.lcd.setCursor(40,0);
    robot.lcd.print(motor);
    
    robot.lcd.setCursor(0,2);
    robot.lcd.print(Pertenencias[LEJOS_ATRAS]);
    robot.lcd.setCursor(22,2);
    robot.lcd.print(Pertenencias[MEDIO_ATRAS]);
    robot.lcd.setCursor(44,2);
    robot.lcd.print(Pertenencias[CERCA_ATRAS]);
    robot.lcd.setCursor(66,2);
    robot.lcd.print(Pertenencias[CORRECTO]);
    robot.lcd.setCursor(0,4);
    robot.lcd.print(Pertenencias[CERCA_ALANTE]);
    robot.lcd.setCursor(22,4);
    robot.lcd.print(Pertenencias[MEDIO_ALANTE]);
    robot.lcd.setCursor(44,4);
    robot.lcd.print(Pertenencias[LEJOS_ALANTE]);
    
          
  }
  if(millis()-timerGP2D>2)
  {
    timerGP2D=millis();
    distancia=robot.gp2dmm(GP2);
    ERROR=CONSIGNA-distancia;
    Fuzzyfication(ERROR);
    motor=(DeFuzzyfication())*-1;
    robot.motores(motor,motor);
  }
}


